/**
  ******************************************************************************
  * @file    driver_motor.h
  * @author  XJTU ROBOMASTER Hero Team
  * @brief   This file contains all the functions prototypes for the motor
  *          driver.
  ******************************************************************************
  * @attention
  *
  * Please add comments after adding or deleting functions to ensure code
	* specification.
  *
  ******************************************************************************
  */

/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef DRIVER_MOTOR_H
#define DRIVER_MOTOR_H

#include "main.h"
#include "can.h"
#include "BSPconfig.h"
#include "pid.h"
#include "ramp.h"


#define MOTOR_PID_MODE_SPEED (0)
#define MOTOR_PID_MODE_LOCATION (1)

#define P_MIN     (-3.14f)
#define P_MAX     (3.14f)
#define V_MIN     (-30.0f)
#define V_MAX     (30.0f)
#define KP_MIN    (0.0f)
#define KP_MAX    (500.0f)
#define KD_MIN    (0.0f)
#define KD_MAX    (5.0f)
#define T_MIN     (-10.0f)
#define T_MAX     (10.0f)
/* Exported types ------------------------------------------------------------*/
/**
  * @brief DM Motor mode enumeration
  */
typedef enum
{
    Motor4310_MIT = 1U,    /*!< MIT模式是为了兼容原版MIT模式所设计,可以在实现无缝切换的同时,能够灵活设
                                定控制范围(P_MAX,V_MAX,T_MAX),电调将接收到的 CAN 数据转化成控制变量进
                                行运算得到扭矩值作为电流环的电流给定,电流环根据其调节规律最终达到给定的扭矩电流 */

    Motor4310_POS = 2U,    /*!< 位置串级模式是采用三环串联控制的模式,位置环作为最外环,其输出作为速度环的给
                                定,而速度环的输出作为内环电流环的给定,用以控制实际的电流输出 */

    Motor4310_VEL = 3U     /*!< 速度模式能让电机稳定运行在设定的速度 */

} DM_MotorMode_Enum;

/**
  * @brief DM Motor state enumeration
  */
typedef enum
{
    MotorDisable = 0U,          /*!< 电机没有异常状态，已经使能并可以正常工作 */

    MotorEnable = 1U,           /*!< 电机初始状态，没有被使能 */

    MotorExit = 2U,             /*!< 关闭电机 */

    MotorOverVoltage = 0x8,     /*!< 电机超压，电压高于32V，过压将退出“使能模式” */

    MotorUnderVoltage = 0x9,    /*!< 电机欠压，电压低于15V，欠压将退出“使能模式” */

    MotorOverCurrent = 0xA,     /*!< 电机过电流，电流高于9.8A，过流将退出“使能模式” */

    MosOverTemperature = 0xB,   /*!< MOS过温，温度大于120℃将退出“使能模式” */

    CoilOverTemperature = 0xC,  /*!< 线圈过温，温度大于120℃将退出“使能模式” */

    CommunicationLos = 0xD,     /*!< 通讯丢失防护，设定周期内没有收到 CAN 指令将自动退出“使能模式” */

    MotorOverload = 0xE         /*!< 电机过载 */

} DM_MotorState_Enum;

typedef struct
{
    uint8_t ReceiveMessege[8];     /*!< CAN receive array */

    int16_t EncoderData;           /*!< The encoder value,
                                    	This parameter must be a number between Min_Data = 0 and Max_Data = 8191. */

    int16_t EncoderDataLast;       /*!< The last encoder value,
                                    	This parameter must be a number between Min_Data = 0 and Max_Data = 8191. */

    int16_t EncoderCount;          /*!< The last encoder value,
                                    	This parameter must be a number between Min_Data = 0 and Max_Data = 8191. */

    int16_t RawSpeed;              /*!< Motor rotor speed,
                                    	If it's a 3508 motor, This parameter should be a number between Min_Data = -9006 and Max_Data = +9006.
                                      If it's a 2006 motor, This parameter should be a number between Min_Data = -14976 and Max_Data = +14976.
                                      If it's a 6020 motor, This parameter should be a number between Min_Data = -320 and Max_Data = +320.	*/
    int16_t Speed_array[10];       /*!< The first ten times motor rotor speed,
                                    	If it's a 3508 motor, This parameter should be a number between Min_Data = -9006 and Max_Data = +9006.
                                      If it's a 2006 motor, This parameter should be a number between Min_Data = -14976 and Max_Data = +14976.
                                      If it's a 6020 motor, This parameter should be a number between Min_Data = -320 and Max_Data = +320.	*/

    int16_t MeanFilterSpeed;       /*!< Motor rotor speed passing through the mean filter,
                                    	If it's a 3508 motor, This parameter should be a number between Min_Data = -9006 and Max_Data = +9006.
                                      If it's a 2006 motor, This parameter should be a number between Min_Data = -14976 and Max_Data = +14976.
                                      If it's a 6020 motor, This parameter should be a number between Min_Data = -320 and Max_Data = +320.	*/

    int16_t LowPassFilterSpeed;    /*!< Motor rotor speed after passing through a first-order low-pass filter,
                                    	If it's a 3508 motor, This parameter should be a number between Min_Data = -9006 and Max_Data = +9006.
                                      If it's a 2006 motor, This parameter should be a number between Min_Data = -14976 and Max_Data = +14976.
                                      If it's a 6020 motor, This parameter should be a number between Min_Data = -320 and Max_Data = +320.	*/

    int16_t RawCurrent;            /*!< Actual torque current */

    int16_t RawTorque;             /*!< Actual output torque */

    int8_t RawTemperature;         /*!< The 3508 or 6020 motor temperature */

    struct{
        float	SetSpeed;              /*!< Speed setting value,
                                      This parameter can be a number between Min_Data = -1 and Max_Data = 1. */

        float	Speed ;                 /*!< Speed values after normalization,
                                      This parameter can be a number between Min_Data = -1 and Max_Data = 1. */
    }Speed;

    struct{
        float	SetLocation;           /*!< Location setting value */

        float	Location;              /*!< Location values after normalization */
    }Location;

    PID PIDSpeed;                  /*!< Speed pid struct */

    PID PIDLocation;               /*!< Location pid struct */

    uint8_t PIDMode;               /*!< PID for speed or location */

    ramp_t Chassis_RAMP;

    float ax;
    float bx;
    float precurrent;
}MotorStruct;

typedef struct
{
    DM_MotorState_Enum MotorState; /*!< State of DM motor. */

    DM_MotorMode_Enum MotorMode;   /*!< Mode of DM motor,
                                        This parameter should be Motor4310_MIT, Motor4310_POS or Motor4310_VEL. */

    uint8_t EnableCount;           /*!< The number of times an enable message is sent when entering the motor */

    uint8_t CAN_ID;                /*!< CAN of the command sent to the motor */

    uint8_t SendMessege[8];        /*!< CAN send array */

    uint8_t ReceiveMessege[8];     /*!< CAN receive array */

    struct
    {
        float SetPosition;         /*!< Position set value,单位为 rad
                                    	This parameter must be a number between Position_Min and Position_Max. */

        float SetVelocity;         /*!< Velocity set value,单位为 rad/s
                                    	This parameter must be a number between Velocity_Min and Velocity_Max. */

        float SetKp;              /*!< Position Kp set value,
                                    	This parameter must be a number between Min_Data = 0.0f and Max_Data = 500.0f. */

        float SetKd;               /*!< Position Kd set value,
                                    	This parameter must be a number between Min_Data = 0.0f and Max_Data = 5.0f. */

        float SetTorque;           /*!< Torque set value,
                                    	This parameter must be a number between Torque_Min and Torque_Max. */

    } SetValue;                    /*!< The set value sent to the motor */

    struct
    {
        uint8_t State;
        uint8_t CAN_ID;
        float Position;
        float Velocity;
        float Torque;
        float MosTemperature;
        float CoilTemperature;
    } RawFeedback;                 /*!< Motor feedback frame data, this frame data id is Master ID */

    struct
    {
        float SetPosition;
        float SetVelocity;
        float SetKp;
        float SetKd;
        float SetTorque;
    } RawControl;                  /*!< Motor control frame data,
                                        If the motor mode is MIT, this frame data id is CAN_ID
                                        If the motor mode is POS, this frame data id is CAN_ID + 100
                                        If the motor mode is VEL, this frame data id is CAN_ID + 200 */

    float LastPosition;            /*!< The last position value,单位为 rad
                                    	This parameter must be a number between Position_Min and Position_Max. */

    int16_t EncoderCount;          /*!< The number of motor turns */

    float Speed_array[10];         /*!< The first ten times motor rotor speed,单位为 rad/s
                                    	This parameter must be a number between Velocity_Min and Velocity_Max. */

    float MeanFilterSpeed;         /*!< Motor rotor speed passing through the mean filter,单位为 rad/s
                                    	This parameter must be a number between Velocity_Min and Velocity_Max. */

    float Speed_MeanFilter_K;      /*!< Mean filter coefficient,
                                        This parameter must be a number between Min_Data = -1 and Max_Data = 1. */

    float LowPassFilterSpeed;      /*!< Motor rotor speed after passing through a first-order low-pass filter,单位为 rad/s
                                    	This parameter must be a number between Velocity_Min and Velocity_Max. */

    struct
    {
        float SetSpeed;              /*!< Speed setting value,
                                          This parameter can be a number between Min_Data = -1 and Max_Data = 1. */

        float Speed;                 /*!< Speed values after normalization,
                                          This parameter can be a number between Min_Data = -1 and Max_Data = 1. */

    } Speed;

    struct
    {
        float SetLocation;           /*!< Location setting value */

        float Location;              /*!< Location values after normalization */

    } Location;

//    FeedForward SpeedFeedForward;

    PID PIDSpeed;                   /*!< Speed pid struct */

    PID PIDLocation;                /*!< Location pid struct */

} DM_MotorStruct;

//数据中的location采用了记圈的方法，小数部分为实际的角度
extern MotorStruct ChassisMotor[5];
extern DM_MotorStruct yawMotor;


uint8_t Motor4310FeedbackFrameDataUpdate(DM_MotorStruct* motor);
void MotorDataUpdate(MotorStruct *motor);
void MotorPIDCalculate(MotorStruct *motor);
#endif //DRIVER_MOTOR_H
